
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_baro_spl06.c
  * @author     baiyang
  * @date       2022-5-31
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "sensor_baro_spl06.h"

#include <stdio.h>
#include <string.h>
#include <stdlib.h>

/*-----------------------------------macro------------------------------------*/
#define SPL06_CHIP_ID                          0x10

#define SPL06_REG_PRESSURE_B2                  0x00    // Pressure MSB Register
#define SPL06_REG_PRESSURE_B1                  0x01    // Pressure middle byte Register
#define SPL06_REG_PRESSURE_B0                  0x02    // Pressure LSB Register
#define SPL06_REG_PRESSURE_START               SPL06_REG_PRESSURE_B2
#define SPL06_PRESSURE_LEN                     3       // 24 bits, 3 bytes
#define SPL06_REG_TEMPERATURE_B2               0x03    // Temperature MSB Register
#define SPL06_REG_TEMPERATURE_B1               0x04    // Temperature middle byte Register
#define SPL06_REG_TEMPERATURE_B0               0x05    // Temperature LSB Register
#define SPL06_REG_TEMPERATURE_START            SPL06_REG_TEMPERATURE_B2
#define SPL06_TEMPERATURE_LEN                  3       // 24 bits, 3 bytes
#define SPL06_REG_PRESSURE_CFG                 0x06    // Pressure config
#define SPL06_REG_TEMPERATURE_CFG              0x07    // Temperature config
#define SPL06_REG_MODE_AND_STATUS              0x08    // Mode and status
#define SPL06_REG_INT_AND_FIFO_CFG             0x09    // Interrupt and FIFO config
#define SPL06_REG_INT_STATUS                   0x0A    // Interrupt and FIFO config
#define SPL06_REG_FIFO_STATUS                  0x0B    // Interrupt and FIFO config
#define SPL06_REG_RST                          0x0C    // Softreset Register
#define SPL06_REG_CHIP_ID                      0x0D    // Chip ID Register
#define SPL06_REG_CALIB_COEFFS_START           0x10
#define SPL06_REG_CALIB_COEFFS_END             0x21

#define SPL06_CALIB_COEFFS_LEN                 (SPL06_REG_CALIB_COEFFS_END - SPL06_REG_CALIB_COEFFS_START + 1)

// TEMPERATURE_CFG_REG
#define SPL06_TEMP_USE_EXT_SENSOR              (1<<7)

// MODE_AND_STATUS_REG
#define SPL06_MEAS_PRESSURE                    (1<<0)  // measure pressure
#define SPL06_MEAS_TEMPERATURE                 (1<<1)  // measure temperature

#define SPL06_MEAS_CFG_CONTINUOUS              (1<<2)
#define SPL06_MEAS_CFG_PRESSURE_RDY            (1<<4)
#define SPL06_MEAS_CFG_TEMPERATURE_RDY         (1<<5)
#define SPL06_MEAS_CFG_SENSOR_RDY              (1<<6)
#define SPL06_MEAS_CFG_COEFFS_RDY              (1<<7)

// INT_AND_FIFO_CFG_REG
#define SPL06_PRESSURE_RESULT_BIT_SHIFT        (1<<2)  // necessary for pressure oversampling > 8
#define SPL06_TEMPERATURE_RESULT_BIT_SHIFT     (1<<3)  // necessary for temperature oversampling > 8

// Don't set oversampling higher than 8 or the measurement time will be higher than 20ms (timer period)
#define SPL06_PRESSURE_OVERSAMPLING            8
#define SPL06_TEMPERATURE_OVERSAMPLING         8

//#define SPL06_OVERSAMPLING_TO_REG_VALUE(n)     (ffs(n)-1)

#define SPL06_PRESSURE_OVERSAMPLING_TO_REG_VALUE            3
#define SPL06_TEMPERATURE_OVERSAMPLING_TO_REG_VALUE         3

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool _init(sensor_baro_spl06 *spl06);
static int32_t _raw_value_scale_factor(uint8_t oversampling);
static void _timer(void *parameter);
static void _update(sensor_baro_backend *baro_backend);
static void _update_temperature(sensor_baro_spl06 *spl06, int32_t temp_raw);
static void _update_pressure(sensor_baro_spl06 *spl06, int32_t press_raw);

/*----------------------------------variable----------------------------------*/
static struct sensor_baro_backend_ops spl06_ops;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_baro_spl06_ctor(sensor_baro_spl06 *spl06, gp_device_t dev)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(spl06, 0, sizeof(sensor_baro_spl06));

    sensor_baro_backend_ctor(&spl06->backend, "spl06");

    spl06_ops = baro_backend_ops;

    spl06_ops.update = _update;

    spl06->backend.ops = &spl06_ops;

    spl06->_dev = dev;
}

void sensor_baro_spl06_destructor(sensor_baro_spl06 *spl06)
{
    sensor_baro_backend_destructor(&spl06->backend);
}

sensor_baro_backend *sensor_baro_spl06_probe(gp_device_t dev)
{
    if (!dev) {
        return NULL;
    }
    sensor_baro_spl06 *sensor = (sensor_baro_spl06 *)rt_malloc(sizeof(sensor_baro_spl06));
    sensor_baro_spl06_ctor(sensor, dev);
    
    if (!sensor || !_init(sensor)) {
        sensor_baro_spl06_destructor(sensor);
        rt_free(sensor);
        return NULL;
    }
    return (sensor_baro_backend *)sensor;
}

static bool _init(sensor_baro_spl06 *spl06)
{
    if (!spl06->_dev) {
        return false;
    }
    //WITH_SEMAPHORE(_dev->get_semaphore());

    devmgr_set_speed(spl06->_dev, DEV_SPEED_HIGH);

    uint8_t whoami;

// Sometimes SPL06 has init problems, that's due to failure of reading using SPI for the first time. The SPL06 is a dual
// protocol sensor(I2C and SPI), sometimes it takes one SPI operation to convert it to SPI mode after it starts up.
    bool is_SPL06 = false;

    for (uint8_t i=0; i<5; i++) {
        if (devmgr_read_registers(spl06->_dev, SPL06_REG_CHIP_ID, &whoami, 1)  &&
            whoami == SPL06_CHIP_ID) {
            is_SPL06=true;
            break;
        }
    }
    
    if(!is_SPL06) {
        return false;
    }

    // read the calibration data
    uint8_t buf[SPL06_CALIB_COEFFS_LEN];
    devmgr_read_registers(spl06->_dev, SPL06_REG_CALIB_COEFFS_START, buf, sizeof(buf));

    spl06->_c0 = (buf[0] & 0x80 ? 0xF000 : 0) | ((uint16_t)buf[0] << 4) | (((uint16_t)buf[1] & 0xF0) >> 4);
    spl06->_c1 = ((buf[1] & 0x8 ? 0xF000 : 0) | ((uint16_t)buf[1] & 0x0F) << 8) | (uint16_t)buf[2];
    spl06->_c00 = (buf[3] & 0x80 ? 0xFFF00000 : 0) | ((uint32_t)buf[3] << 12) | ((uint32_t)buf[4] << 4) | (((uint32_t)buf[5] & 0xF0) >> 4);
    spl06->_c10 = (buf[5] & 0x8 ? 0xFFF00000 : 0) | (((uint32_t)buf[5] & 0x0F) << 16) | ((uint32_t)buf[6] << 8) | (uint32_t)buf[7];
    spl06->_c01 = ((uint16_t)buf[8] << 8) | ((uint16_t)buf[9]);
    spl06->_c11 = ((uint16_t)buf[10] << 8) | (uint16_t)buf[11];
    spl06->_c20 = ((uint16_t)buf[12] << 8) | (uint16_t)buf[13];
    spl06->_c21 = ((uint16_t)buf[14] << 8) | (uint16_t)buf[15];
    spl06->_c30 = ((uint16_t)buf[16] << 8) | (uint16_t)buf[17];

    // setup temperature and pressure measurements
    devmgr_setup_checked_registers(spl06->_dev, 3, 20);

    devmgr_write_register(spl06->_dev, SPL06_REG_TEMPERATURE_CFG, SPL06_TEMP_USE_EXT_SENSOR | SPL06_TEMPERATURE_OVERSAMPLING_TO_REG_VALUE, true);
    devmgr_write_register(spl06->_dev, SPL06_REG_PRESSURE_CFG, SPL06_PRESSURE_OVERSAMPLING_TO_REG_VALUE, true);
    
    uint8_t int_and_fifo_reg_value = 0;
    if (SPL06_TEMPERATURE_OVERSAMPLING > 8) {
        int_and_fifo_reg_value |= SPL06_TEMPERATURE_RESULT_BIT_SHIFT;
    }
    if (SPL06_PRESSURE_OVERSAMPLING > 8) {
        int_and_fifo_reg_value |= SPL06_PRESSURE_RESULT_BIT_SHIFT;
    }
    devmgr_write_register(spl06->_dev, SPL06_REG_INT_AND_FIFO_CFG, int_and_fifo_reg_value, true);

    spl06->_instance = sensor_baro_register_sensor();

    devmgr_set_device_type(spl06->_dev, DEVTYPE_BARO_SPL06);
    sensor_baro_backend_set_bus_id(spl06->_instance, devmgr_get_bus_id(spl06->_dev));

    // request 50Hz update
    spl06->_timer_counter = -1;
    devmgr_register_periodic_callback(spl06->_dev, 20 * AP_USEC_PER_MSEC, _timer, spl06);

    return true;
}

static int32_t _raw_value_scale_factor(uint8_t oversampling)
{
    // From the datasheet page 13
    switch(oversampling)
    {
        case 1: return 524288;
        case 2: return 1572864;
        case 4: return 3670016;
        case 8: return 7864320;
        case 16: return 253952;
        case 32: return 516096;
        case 64: return 1040384;
        case 128: return 2088960;
        default: return -1; // invalid
    }
}

// acumulate a new sensor reading
static void _timer(void *parameter)
{
    sensor_baro_spl06 *spl06 = (sensor_baro_spl06 *)parameter;

    uint8_t buf[3];

    if ((spl06->_timer_counter == -1) || (spl06->_timer_counter == 49)) {
        // First call and every second start a temperature measurement (50Hz call)
        devmgr_write_register(spl06->_dev, SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_TEMPERATURE, false);
        spl06->_timer_counter = 0; // Next cycle we are reading the temperature
    } else if (spl06->_timer_counter == 0) {
        // A temperature measurement had been started during the previous call
        devmgr_read_registers(spl06->_dev, SPL06_REG_TEMPERATURE_START, buf, sizeof(buf));
        _update_temperature(spl06, (int32_t)((buf[0] & 0x80 ? 0xFF000000 : 0) | ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | buf[2]));
        devmgr_write_register(spl06->_dev, SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_PRESSURE, false);
        spl06->_timer_counter += 1;
    } else {
        // The rest of the time read the latest pressure and start a new measurement
        devmgr_read_registers(spl06->_dev, SPL06_REG_PRESSURE_START, buf, sizeof(buf));
        _update_pressure(spl06, (int32_t)((buf[0] & 0x80 ? 0xFF000000 : 0) | ((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | buf[2]));
        devmgr_write_register(spl06->_dev, SPL06_REG_MODE_AND_STATUS, SPL06_MEAS_PRESSURE, false);
        spl06->_timer_counter += 1;
    }

    devmgr_check_next_register(spl06->_dev);
}

// transfer data to the frontend
static void _update(sensor_baro_backend *baro_backend)
{
    sensor_baro_spl06 *spl06 = (sensor_baro_spl06 *)baro_backend;

    rt_mutex_take(spl06->backend._mutex, RT_WAITING_FOREVER);

    if (spl06->_pressure_count == 0) {
        rt_mutex_release(spl06->backend._mutex);
        return;
    }

    sensor_baro_backend_copy_to_frontend(spl06->_instance, spl06->_pressure_sum/spl06->_pressure_count, spl06->_temperature);

    spl06->_pressure_sum = 0;
    spl06->_pressure_count = 0;

    rt_mutex_release(spl06->backend._mutex);
}

// calculate temperature
static void _update_temperature(sensor_baro_spl06 *spl06, int32_t temp_raw)
{
    spl06->_temp_raw = (float)temp_raw / _raw_value_scale_factor(SPL06_TEMPERATURE_OVERSAMPLING);
    const float temp_comp = (float)spl06->_c0 / 2 + spl06->_temp_raw * spl06->_c1;

    rt_mutex_take(spl06->backend._mutex, RT_WAITING_FOREVER);

    spl06->_temperature = temp_comp;

    rt_mutex_release(spl06->backend._mutex);
}

// calculate pressure
static void _update_pressure(sensor_baro_spl06 *spl06, int32_t press_raw)
{
    const float press_raw_sc = (float)press_raw / _raw_value_scale_factor(SPL06_PRESSURE_OVERSAMPLING);
    const float pressure_cal = (float)spl06->_c00 + press_raw_sc * ((float)spl06->_c10 + press_raw_sc * ((float)spl06->_c20 + press_raw_sc * spl06->_c30));
    const float press_temp_comp = spl06->_temp_raw * ((float)spl06->_c01 + press_raw_sc * ((float)spl06->_c11 + press_raw_sc * spl06->_c21));

    const float press_comp = pressure_cal + press_temp_comp;

    if (!sensor_baro_backend_pressure_ok(&spl06->backend, press_comp)) {
        return;
    }

    rt_mutex_take(spl06->backend._mutex, RT_WAITING_FOREVER);

    spl06->_pressure_sum += press_comp;
    spl06->_pressure_count++;

    rt_mutex_release(spl06->backend._mutex);
}

/*------------------------------------test------------------------------------*/


